import json
import os
import sys
import time
from datetime import datetime

import airsim
import numpy as np
from PyQt5.QtWebEngineWidgets import QWebEngineView
from PyQt5 import uic, QtCore
from PyQt5.QtCore import QUrl, QThread, pyqtSignal
from PyQt5.QtWidgets import QMainWindow, QApplication, QWidget, QTableWidgetItem
from airsim import Pose, Vector3r

from utils.rabbitmq import RabbitMqThread
import pymap3d as pm
from utils.getmultirotorstate import GetMultirotorState


# 主窗口类，继承自QMainWindow
class Window(QMainWindow):
    def __init__(self):
        super().__init__()
        # 获取当前工作目录
        self.current_working_directory = os.getcwd()
        # 加载UI文件
        self.ui = uic.loadUi('UI/window.ui', self)

        # 初始化UI控件引用
        self.web_browser = self.ui.webBrowser  # 地图浏览器
        self.uav_number_select = self.ui.uav_number  # 无人机选择下拉框
        self.uav_status = self.ui.uav_status  # 无人机状态显示
        self.fly_button = self.ui.fly  # 起飞按钮
        self.air_line = self.ui.air_line  # 航线按钮
        self.reset_button = self.ui.reset  # 重置按钮
        self.test_model_select = self.ui.test_model  # 测试模式选择
        self.latitude_line = self.ui.latitude  # 高度显示
        self.horizon_line = self.ui.horizon  # 水平速度显示
        self.vertical_line = self.ui.vertical  # 垂直速度显示

        # 初始化航线窗口
        self.waypoint_window = WaypointWindow()

        # 创建AirSim客户端
        self.client = airsim.MultirotorClient()

        # 初始化数据结构
        self.vehicles = []  # 所有无人机名称列表
        self.vehicles_status = {}  # 无人机状态字典
        self.waypoints = {}  # 无人机航线信息字典
        self.uavs_latlng = {}  # 无人机实时经纬度字典

        # 当前选择的无人机
        self.current_uav_number_select = None

        #TODO 可以读取setting.json文件里的OriginGeopoint参数
        self.initial_latlng = {"lat": 22.2920, "lon": 113.5765, "alt": 0}  # 初始位置，需要与Airsim的settings.json文件里的OriginGeopoint一致，

        # 当前测试模式
        self.current_test_model_select = None

        # 模式选项
        self.test_select_models = ['无', '偏航']  # 测试模式选项

        # 初始化设置
        self.setting_init()
        self.MAX_SPEED=20
        # 连接按钮信号
        self.fly_button.clicked.connect(self.fly_button_clicked)
        self.air_line.clicked.connect(self.air_line_clicked)
        self.reset_button.clicked.connect(self.reset_button_clicked)

        # 初始化RabbitMQ线程
        self.rabbitmq_thread = RabbitMqThread()
        self.rabbitmq_thread.rabbit_signal.connect(self.receive_rabbitmq)
        self.rabbitmq_thread.start()

        # 初始化起飞监听线程
        self.listen_takeoff_thread = ListenTakeoffTime(self.MAX_SPEED)
        self.listen_takeoff_thread.takeoff_message.connect(self.receive_takeoff_message)
        self.listen_takeoff_thread.start()

        # 初始化无人机状态获取线程
        self.getmultirotor_state_thread = GetMultirotorState(self.vehicles)
        self.getmultirotor_state_thread.multirotorstate_signal.connect(self.receive_getmultirotor_state)
        self.getmultirotor_state_thread.start()

        # 连接下拉框信号
        self.uav_number_select.currentIndexChanged.connect(self.uav_number_changed)
        self.test_model_select.currentIndexChanged.connect(self.test_model_changed)

    def air_line_clicked(self):
        """显示当前选择无人机的航线信息窗口"""
        routes = self.waypoints[self.current_uav_number_select]
        self.waypoint_window.load_message(routes)
        self.waypoint_window.show()

    def receive_takeoff_message(self, message):
        """接收起飞消息，更新无人机状态"""
        self.vehicles_status[message] = "飞行中"

    def fly_button_clicked(self):
        """起飞按钮点击事件处理"""
        if self.vehicles_status[self.current_uav_number_select] != "飞行中":
            # 通知起飞监听线程立即起飞
            self.listen_takeoff_thread.takeoff_now(self.current_uav_number_select)
            # 更新状态显示
            self.uav_status.setText(self.vehicles_status[self.current_uav_number_select])

    def reset_button_clicked(self):
        """重置按钮点击事件处理"""
        self.client.reset()  #TODO 重置AirSim仿真环境，但是还没有想法怎么实施

    def receive_getmultirotor_state(self, message):
        """接收无人机状态信息"""
        # 更新当前选择无人机的状态显示
        altitude = message[self.current_uav_number_select]["altitude"]
        horizon = message[self.current_uav_number_select]["horizon"]
        vertical = -message[self.current_uav_number_select]["vertical"]
        self.latitude_line.setText(format(altitude, '.2f'))
        self.horizon_line.setText(format(horizon, '.2f'))
        self.vertical_line.setText(format(vertical, '.2f'))

        # 更新所有无人机的位置信息
        for number in list(message.keys()):
            self.uavs_latlng[number] = message[number]["latlng"]
            # 如果无人机在飞行中，更新地图上的位置
            if self.vehicles_status[number] == "飞行中":
                try:
                    latlng = message[number]["latlng"]
                    javascript = f"iconMoving({latlng},'{number}')"
                    self.web_browser.page().runJavaScript(javascript)
                except Exception as e:
                    print(e)

    def setting_init(self):
        """初始化设置"""
        # 加载地图HTML文件
        local_html_path = self.current_working_directory + "\\map.html"
        self.web_browser.load(QUrl.fromLocalFile(local_html_path))

        # 初始化测试模式下拉框
        for select_model in self.test_select_models:
            self.test_model_select.addItem(select_model)
        self.test_model_select.setCurrentIndex(0)

    def uav_number_changed(self, index):
        """无人机选择下拉框变化事件处理"""
        self.current_uav_number_select = self.vehicles[index]
        # 在地图上高亮显示选中的无人机
        javascript = f"layersAnimatedMarkers['{self.current_uav_number_select}'].openPopup();"
        self.web_browser.page().runJavaScript(javascript)
        # 更新状态显示
        self.uav_status.setText(self.vehicles_status[self.current_uav_number_select])

    def plot_lead_line(self, waypoints):
        """绘制引导路线"""
        # 创建一个虚拟无人机用于绘制路线
        pose = Pose(Vector3r(0, 0, 0))
        number = "0000000"
        self.client.simAddVehicle(number, "simpleflight", pose)

        # 初始化起点位置
        pos_start = np.array([[0.0], [0.0], [0.0]])
        plot_last_pos = [airsim.Vector3r(pos_start[0, 0], pos_start[1, 0], pos_start[2, 0])]

        # 遍历所有航点
        for waypoint in waypoints:
            lat = waypoint["lat"]
            lon = waypoint["lon"]
            alt = waypoint["alt"]
            initial_lat = self.initial_latlng["lat"]
            initial_lon = self.initial_latlng["lon"]

            # 计算NED坐标
            e, n, u = pm.geodetic2ned(lat, lon, 0, initial_lat, initial_lon, 0)
            total_e = 0.0
            total_n = 0.0

            # 逐步移动到目标位置
            while abs(e) > 0.5 or abs(n) > 0.5:
                total_e += e.item()
                total_n += n.item()
                pose = Pose(Vector3r(total_e, total_n, 0))
                self.client.simSetVehiclePose(pose, False, number)
                time.sleep(0.1)
                state = self.client.getMultirotorState(number)
                initial_lat = state.gps_location.latitude
                initial_lon = state.gps_location.longitude
                e, n, u = pm.geodetic2ned(lat, lon, 0, initial_lat, initial_lon, 0)

            # 绘制路线段
            temp = [total_e, total_n, -alt]
            pos_now = np.array([[temp[0]], [temp[1]], [temp[2]]])
            plot_v_start = [airsim.Vector3r(pos_now[0, 0], pos_now[1, 0], pos_now[2, 0])]
            self.client.simPlotLineList(plot_last_pos + plot_v_start,
                                        color_rgba=[0.0, 1.0, 0.0, 1.0],
                                        thickness=15, is_persistent=True)
            plot_last_pos = plot_v_start

    def test_model_changed(self, index):
        """测试模式选择变化事件处理"""
        self.current_test_model_select = self.test_select_models[index]

    def receive_rabbitmq(self, message):
        """接收RabbitMQ消息"""
        print(message)
        message = json.loads(message)
        pattern = message['pattern']  # 消息模式 (0=新航线, 1=修改航线)
        number = message['number']  # 无人机编号
        routes = message['routes']  # 航线点列表
        self.waypoints[number] = message  # 存储航线信息

        # 特殊处理引导无人机，因为Airsim只能固定一个初始无人机的视角，看不到其他无人机的视角
        if number == "000":
            self.plot_lead_line(routes)  # 绘制引导路线
            self.vehicles.append(number)  # 添加到无人机列表
            self.vehicles_status[number] = "未解锁"  # 设置状态
            self.uav_number_select.addItem(number)  # 添加到下拉框

        # 解析起飞时间
        fly_time = datetime.strptime(message['fly_time'], "%Y-%m-%d %H:%M:%S")

        # 准备地图航线数据
        mapRoutes = [[float(point['lat']), float(point['lon'])] for point in routes]
        javascript = f"ployInitialRouteLine({mapRoutes},{pattern},'{number}')"

        # 新航线命令
        if pattern == 0:
            if number != "000":  # 非引导无人机
                self.generate_uav(routes[0], number)  # 生成无人机
                # 添加到起飞监听
                self.listen_takeoff_thread.add_uav(number, fly_time, routes)
        # 修改航线命令
        else:
            # 添加当前位置作为起点
            mapRoutes.insert(0, self.uavs_latlng[number])
            # 通知起飞监听线程修改航线
            self.listen_takeoff_thread.change_waypoint(number, routes)

        # 在地图上绘制航线
        self.web_browser.page().runJavaScript(javascript)

    def generate_uav(self, route, number):
        """在指定位置生成无人机"""
        lat = route["lat"]
        lon = route["lon"]
        alt = 0
        initial_lat = self.initial_latlng["lat"]
        initial_lon = self.initial_latlng["lon"]

        # 创建无人机
        pose = Pose(Vector3r(3, 3, 0))
        self.client.simAddVehicle(number, "simpleflight", pose)

        # 计算NED坐标
        e, n, u = pm.geodetic2ned(lat, lon, alt, initial_lat, initial_lon, 0)
        total_e = e.item()
        total_n = n.item()
        total_u = u.item()

        # 逐步移动到目标位置
        while abs(e) > 0.5 or abs(n) > 0.5 or abs(u) > 0.5:
            pose = Pose(Vector3r(total_e, total_n, total_u))
            self.client.simSetVehiclePose(pose, False, number)
            time.sleep(0.1)
            state = self.client.getMultirotorState(vehicle_name=number)
            initial_lat = state.gps_location.latitude
            initial_lon = state.gps_location.longitude
            initial_alt = state.gps_location.altitude
            e, n, u = pm.geodetic2ned(lat, lon, alt, initial_lat, initial_lon, initial_alt)
            total_e += e.item()
            total_n += n.item()
            total_u += u.item()

        # 添加到无人机列表
        self.vehicles.append(number)
        self.vehicles_status[number] = "未解锁"  # 设置初始状态
        self.uav_number_select.addItem(number)  # 添加到下拉框


# 航线信息窗口类
class WaypointWindow(QWidget):
    def __init__(self):
        super().__init__()
        # 加载UI文件
        self.ui = uic.loadUi('UI/waypoint.ui', self)

        # 初始化UI控件引用
        self.uav_number = self.ui.uav_number  # 无人机编号显示
        self.fly_time = self.ui.fly_time  # 起飞时间显示
        self.table_widget = self.ui.table_widget  # 航线点表格

        # 表格列名映射
        self.col_index_line = ["lon", "lat", "alt"]

    def load_message(self, message):
        """加载航线信息到窗口"""
        number = message['number']  # 无人机编号
        routes = message['routes']  # 航线点列表
        fly_time = message['fly_time']  # 起飞时间

        # 设置无人机编号和起飞时间
        self.uav_number.setText(number)
        self.fly_time.setText(fly_time)

        # 清空表格
        self.table_widget.setRowCount(0)

        # 填充航线点数据到表格
        for row_index, coordinate in enumerate(routes):
            self.table_widget.insertRow(row_index)
            for col_index in range(self.table_widget.columnCount()):
                # 获取对应列的数据
                value = coordinate[self.col_index_line[col_index]]
                item = QTableWidgetItem(str(value))
                self.table_widget.setItem(row_index, col_index, item)


# 起飞监听线程类
class ListenTakeoffTime(QThread):
    # 定义起飞信号
    takeoff_message = pyqtSignal(str)

    def __init__(self, velocity):
        super(ListenTakeoffTime, self).__init__()
        # 初始化数据结构
        self.timetable = {}  # 起飞时间表 {无人机编号: 起飞时间}
        self.number_count = {}  # 航线修改次数计数器
        self.waypoints = {}  # 航线点字典
        self.velocity = velocity  # 飞行速度
        self.uav_flys = {}  # 无人机飞行线程字典

    def takeoff_now(self, number):
        """立即起飞指定无人机"""
        self.timetable[number] = datetime.now()

    def add_uav(self, number, takeoff_time, waypoint):
        """添加无人机到监听列表"""
        self.timetable[number] = takeoff_time
        self.number_count[number] = 0
        self.waypoints[number] = waypoint

    def change_waypoint(self, number, waypoint):
        """修改无人机的航线"""
        self.waypoints[number] = waypoint
        self.number_count[number] += 1
        self.timetable[number] = datetime.now()

    def run(self):
        """线程主循环"""
        while True:
            # 检查所有待起飞无人机
            while self.timetable:
                for number, takeoff_time in list(self.timetable.items()):
                    current_datetime = datetime.now()
                    # 检查是否到达起飞时间
                    if takeoff_time <= current_datetime:
                        # 创建并启动飞行线程
                        self.uav_flys[number] = UavFly(number, self.waypoints[number],
                                                       self.velocity, self.number_count[number])
                        self.uav_flys[number].start()
                        # 发送起飞信号
                        self.takeoff_message.emit(number)
                        # 从监听列表中移除
                        self.timetable.pop(number)
                        self.waypoints.pop(number)
                # 每秒检查一次
                time.sleep(1)


# 无人机飞行控制线程类
class UavFly(QThread):
    def __init__(self, number, waypoints, velocity, count):
        super(UavFly, self).__init__()
        # 创建AirSim客户端
        self.client = airsim.MultirotorClient()
        # 启用API控制
        self.client.enableApiControl(True, number)
        # 解锁无人机
        self.client.armDisarm(True, number)
        # 起飞
        self.client.takeoffAsync(number).join()

        # 设置参数
        self.number = number  # 无人机编号
        self.waypoints = waypoints  # 航线点列表
        self.velocity = velocity  # 飞行速度
        self.count = count  # 航线修改次数

    def run(self):
        """线程主函数"""
        # 如果是修改航线，取消之前的任务
        if self.count != 0:
            self.client.cancelLastTask(vehicle_name=self.number)

        # 遍历所有航点
        for waypoint in self.waypoints:
            lat = waypoint['lat']  # 纬度
            lon = waypoint['lon']  # 经度
            alt = waypoint['alt']  # 高度
            # 移动到目标位置
            self.client.moveToGPSAsync(lat, lon, alt,
                                       velocity=self.velocity,
                                       vehicle_name=self.number).join()


# 主程序入口
if __name__ == '__main__':
    # 启用高DPI缩放
    QtCore.QCoreApplication.setAttribute(QtCore.Qt.AA_EnableHighDpiScaling)

    # 创建应用和主窗口
    app = QApplication(sys.argv)
    window = Window()
    window.show()

    # 启动应用主循环
    sys.exit(app.exec_())
